/*
 * Copyright (c)	2010	Bo Zhou<Bo.Schwarzstein@gmail.com>
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 
		http://www.apache.org/licenses/LICENSE-2.0 
 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "BilateralMeshSmoothing.h"

BilateralMeshSmoothing::BilateralMeshSmoothing(const MObject& MeshObject) : MeshSmoothing(MeshObject), mSigmaC(0.1f), mSigmaS(0.1f)
{
}

BilateralMeshSmoothing::~BilateralMeshSmoothing()
{
}

void BilateralMeshSmoothing::SetSigma(const float SigmaC, const float SigmaS)
{
	mSigmaC = SigmaC;
	mSigmaS = SigmaS;
}

void BilateralMeshSmoothing::GetSigma(float& SigmaC, float& SigmaS)
{
	SigmaC = mSigmaC;
	SigmaS = mSigmaS;
}

bool BilateralMeshSmoothing::Smooth()
{
	const float SigmaC = mSigmaC;
	const float SigmaS = mSigmaS;

	const int VertexCount = mMesh.numVertices();
	MPointArray NewPoints;
	mMesh.getPoints(NewPoints);

	std::valarray<int> VertexIds( VertexCount );
	for(size_t i=0; i<VertexIds.size(); ++i)
	{
		VertexIds[i] = (int)i;
	}

	if( IsUsingPaintedVertices() )
	{
		VertexIds = mPaintedVertices;
	}

	for(size_t i=0; i<VertexIds.size(); ++i)
	{
		const int vid = VertexIds[i];

		std::set<int> Neighbors;
		int* p = &mVertexSharedPolygons[vid*(mMaxVertexSharedTime+1)];
		while( *p > -1 )
		{
			const int Polygon = *p;
			MIntArray IndicesOnPolygon;
			mMesh.getPolygonVertices(Polygon, IndicesOnPolygon);
			for(unsigned int j=0; j<IndicesOnPolygon.length(); ++j)
			{
				Neighbors.insert( IndicesOnPolygon[j] );
			}
			IndicesOnPolygon.clear();
			IndicesOnPolygon.setLength(0);
			++p;
		}
		std::set<int>::iterator itr = Neighbors.find(vid);
		assert( itr != Neighbors.end() );
		Neighbors.erase(itr);

		MPoint CentralPoint;
		mMesh.getPoint(vid,CentralPoint);
		MVector CentralNormal;
		mMesh.getVertexNormal(vid,CentralNormal);
		CentralNormal.normalize();

		double Sum = 0.0;
		double Normalizer = 0.0;
		for(itr=Neighbors.begin(); itr!=Neighbors.end(); ++itr)
		{
			MPoint NeighborPoint;
			mMesh.getPoint(*itr, NeighborPoint);

			MVector  E = NeighborPoint - CentralPoint;

			double t = E.length();

			double h = E.x*CentralNormal.x + E.y*CentralNormal.y + E.z*CentralNormal.z;
			double OmegaC = exp(-t*t/(2.0*SigmaC*SigmaC));
			double OmegaS = exp(-t*t/(2.0*SigmaS*SigmaS));

			Sum += OmegaC*OmegaS*h;
			Normalizer += OmegaC*OmegaS;
		}

		NewPoints[vid] = CentralPoint + CentralNormal*( Sum/Normalizer );
	}

	mMesh.setPoints(NewPoints);
	
	NewPoints.clear();
	NewPoints.setLength(0);

	return true;
}
